#include "math.h"
#include "User_define.h"

void MPU_getMotion6(float *,float *,float *,float *,float *,float *);
unsigned long millis();






void MyIMU_AHRSupdate( double *ypr1,double *ypr2,double *ypr3)
{
   float ax=0; 
   float ay=0;
   float az=0;
   float gx=0;
   float gy=0;
   float gz=0;
   float q0 = 1.0;
   float q1 = 0.0;
   float q2 = 0.0;
   float q3 = 0.0;
   float norm = 0;
   float exAcc=0;
   float eyAcc=0;
   float ezAcc=0;
   float vx=0;
   float vy=0;
   float vz=0;
   float exInt=0;
   float eyInt=0;
   float ezInt=0;

   float halfT;
   unsigned long lastUpdate = 0;
   unsigned long now1 = 0;
	now1 = millis();
	halfT = (now1 - lastUpdate) / 2000.0;	// chuyen sang sec
	lastUpdate = now1;
//	deltaT = 2 * halfT;						// dung cho tich phan PID
   MPU_getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  /****************************************
                AHRS filter
  ****************************************/

	// normalise the measurements
	norm = sqrt(ax * ax + ay * ay + az * az); 
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;
	// estimated direction of gravity and flux (v and w)
	vx = 2 * (q1*q3 - q0*q2);
	vy = 2 * (q0*q1 + q2*q3);
	vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

	exAcc = ay * vz - az * vy;
	eyAcc = az * vx - ax * vz;
	ezAcc = ax * vy - ay * vx;
    
//	exMag = my * wz - mz * wy;
//	eyMag = mz * wx - mx * wz;
//	ezMag = mx * wy - my * wx;

	// integral error scaled integral gain
	exInt += exAcc * Ki;
	eyInt += eyAcc * Ki;
	ezInt += ezAcc * Ki;
//	exInt += exAcc * kiAcc + exMag * kiMag;
//	eyInt += eyAcc * kiAcc + eyMag * kiMag;
//	ezInt += ezAcc * kiAcc + ezMag * kiMag;

	// adjusted gyroscope measurements
	gx += exAcc * Kp + exInt;
	gy += eyAcc * Kp + eyInt;
	gz += ezAcc * Kp + ezInt;
//	gx = gx + exAcc * kpAcc + exMag * kpMag + exInt;
//	gy = gy + eyAcc * kpAcc + eyMag * kpMag + eyInt;
//	gz = gz + ezAcc * kpAcc + ezMag * kpMag + ezInt;

	// integrate quaternion rate and normalise
	q0 += (-q1 * gx - q2 * gy - q3 * gz) * halfT;
	q1 += (q0 * gx + q2 * gz - q3 * gy) * halfT;
	q2 += (q0 * gy - q1 * gz + q3 * gx) * halfT;
	q3 += (q0 * gz + q1 * gy - q2 * gx) * halfT;

	// normalise quaternion
	norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;

    q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;

    gx = 2 * (q[1] * q[3] - q[0] * q[2]);
	gy = 2 * (q[0] * q[1] + q[2] * q[3]);
	gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

    *ypr1 = atan2(2 * (q[1] * q[2] - q[0] * q[3]), 2 * (q[0] * q[0] + q[1] * q[1]) - 1);	// YAW
	*ypr2 = atan(gx / sqrt(gy * gy + gz * gz)) ;											// PITCH
	*ypr3 = atan(gy / sqrt(gx * gx + gz * gz)) ;                                           // ROLL


}
void MPU_getMotion6(float *ax,float *ay,float *az,float *gx,float *gy,float *gz)
{
      int i;
      float x,y,z;
      float gOffsets1=0.0;
      float gOffsets2=0.0;
      float gOffsets3=0.0;
     
      *ax = (((AdcRegs.ADCRESULT4>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_X_AXIS)*10;
      *ay = (((AdcRegs.ADCRESULT6>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Y_AXIS)*10;
      *az = (((AdcRegs.ADCRESULT5>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Z_AXIS)*10;

      x =(((((AdcRegs.ADCRESULT2>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD) /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_3 pitch
	  y =(((((AdcRegs.ADCRESULT1>>4) /*- ZOFFSET/3*/ ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_1 roll
	  z =(((((AdcRegs.ADCRESULT3>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_2 yaw;

      for( i=0;i<64;i++)
       {

	     gOffsets1=(gOffsets1*i+x)/(i+1);
	     gOffsets2=(gOffsets2*i+y)/(i+1);
	     gOffsets3=(gOffsets3*i+z)/(i+1);
       }

      *gx = x - gOffsets1;//pitch
	  *gy = y - gOffsets2;//roll
	  *gz = z - gOffsets3;//yaw  
}
unsigned long millis()
{

 int m;
 m = timer0_millis;
 return m;
 }	 